using System;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// Models movement using velocities as inputs and allows sampling of possible errors using
    /// gaussian noise
    /// </summary>
    public class VelocityMotionModel : IMotionModel
    {
        #region Private Members
        private double _tErr1;
        private double _tErr2;
        private double _rErr1;
        private double _rErr2;
        private double _tErr3;
        private double _rErr3;
        private Velocity _vel;
        private double _time;
        private double _v_range;
        private double _w_range;
        private double _g_range;
        private double _v_max;
        private double _w_max;
        private double _g_max;
//private ProbabilityUtils ProbabilityUtils;
        private TrigUtils _trigUtils;
        #endregion

        #region Public Members
        public double tErr1
        {
            get { return _tErr1; }
            set { _tErr1 = value; }
        }

        public double tErr2
        {
            get { return _tErr2; }
            set { _tErr2 = value; }
        }

        public double rErr1
        {
            get { return _rErr1; }
            set { _rErr1 = value; }
        }

        public double rErr2
        {
            get { return _rErr2; }
            set { _rErr2 = value; }
        }

        public double tErr3
        {
            get { return _tErr3; }
            set { _tErr3 = value; }
        }

        public double rErr3
        {
            get { return _rErr3; }
            set { _rErr3 = value; }
        }
        #endregion

        /// <summary>
        /// Models movement using velocities as inputs and allows sampling of possible errors using
        /// gaussian noise.
        /// </summary>
        /// <param name="terr1">Translation Error 1</param>
        /// <param name="terr2">Translation Error 2</param>
        /// <param name="terr3">Translation Error 3</param>
        /// <param name="rerr1">Rotation Error 1</param>
        /// <param name="rerr2">Rotation Error 2</param>
        /// <param name="rerr3">Rotation Error 3</param>
        public VelocityMotionModel(double terr1,double terr2,double terr3,double rerr1,double rerr2,double rerr3)
        {
            
          //  ProbabilityUtils = new ProbabilityUtils();
            _trigUtils = new TrigUtils();
            tErr1 = terr1;
            tErr2 = terr2;
            tErr3 = terr3;
            rErr1 = rerr1;
            rErr2 = rerr2;
            rErr3 = rerr3;
        }

        /// <summary>
        /// Precalculates some variable to make sampling quicker
        /// </summary>
        /// <param name="velocityOfAction">The velocity</param>
        /// <param name="timeOfAction">The length of time</param>
        public void PrepareForSampling(Velocity velocityOfAction, TimeSpan timeOfAction)
        {
            _vel = velocityOfAction;
            _time = timeOfAction.TotalSeconds;
            _v_range = tErr1 * Math.Abs(_vel.Translation * _time) + tErr2 * Math.Abs(_vel.Rotation* _time);
            _w_range = rErr1 * Math.Abs(_vel.Translation * _time) + rErr2 * Math.Abs(_vel.Rotation* _time);
            _g_range = tErr3 * Math.Abs(_vel.Translation * _time) + rErr3 * Math.Abs(_vel.Rotation* _time);
            _v_max = ProbabilityUtils.ProbabilityOfValInVariance(0, _v_range);
            _w_max = ProbabilityUtils.ProbabilityOfValInVariance(0, _w_range);
            _g_max = ProbabilityUtils.ProbabilityOfValInVariance(0, _g_range);
        }


        /// <summary>
        /// Applies the pure motion model with no added noise
        /// </summary>
        /// <param name="state">The current state</param>
        /// <param name="vel">The current velocity</param>
        /// <param name="t">The time for which this should be applied</param>
        /// <returns>The new state</returns>
        public Pose2D GetPerfectSample(Pose2D state, Velocity vel, TimeSpan t)
        {
           // Console.Out.WriteLine("Get perfect sample, received " + state.ToString() + " " + vel.ToString());

            double time = t.TotalSeconds;
            double v = vel.Translation * time;
            double w = vel.Rotation * time;

            // PROBLEM WHEN ROTATION IS CLOSE TO ZERO
            if (Math.Abs(w) < 0.01)
            {
                //double d = vel.Translation * time;
                double x = state.X + v * Math.Cos(state.Theta);
                double y = state.Y + v * Math.Sin(state.Theta);
                return new Pose2D(x, y, state.Theta);
            }
            else
            {
                double factor = v / w;
                double x = state.X - (factor * Math.Sin(state.Theta)) + (factor * Math.Sin(state.Theta + w /* time*/));
                double y = state.Y + (factor * Math.Cos(state.Theta)) - (factor * Math.Cos(state.Theta + w /* time*/));
                double theta = state.Theta + w /* time*/;

                if (theta >= Math.PI) { theta -= Math.PI * 2; }
                else if (theta <= -Math.PI) { theta += Math.PI * 2; };
                //Console.Out.WriteLine("Get perfect sample, returning " + x + "," + y + "," + theta);
                return new Pose2D(x, y, theta);
            }
        }

        /// <summary>
        /// Samples from the latest velocity using the motion error parameters
        /// </summary>
        /// <param name="state">The state that you want to be moved</param>
        /// <returns>The transformed state</returns>
        public Pose2D GetSample(Pose2D state)
        {
            if (_vel == null)
            {
                Console.Out.WriteLine("GridSlam: Motion model has a null velocity. Call robot.Move(vel) first.");
                return state;
            }
            double v = (_vel.Translation * _time) + ProbabilityUtils.RandomFromGaussian(_v_range);
            double w = (_vel.Rotation * _time) + ProbabilityUtils.RandomFromGaussian(_w_range);
            double g = ProbabilityUtils.RandomFromGaussian(_g_range);
            double factor = v/w;
            double x = state.X - (factor * Math.Sin(state.Theta)) + (factor * Math.Sin(state.Theta + w /* _time*/));
            double y = state.Y + (factor * Math.Cos(state.Theta)) - (factor * Math.Cos(state.Theta + w /* _time*/));
            double theta = state.Theta + (w+g) /*_time*/;
           // if (w < 0.01)
           // {
           //     Console.Out.WriteLine("getsample  " + state.ToString() + x + "," + y + "," + theta);
           // }
            // fudge theta
            if (theta >= Math.PI) { theta -= Math.PI*2; } 
            else if (theta <= -Math.PI) { theta += Math.PI*2; };

           // Console.Out.WriteLine("Was: (" + state.X + "," + state.Y + "," + state.Theta + ") Now: (" + x + "," + y + "," + theta + ")");
            return new Pose2D(x,y,theta);
        }
        
       
        /// <summary>
        /// Calculates how likely a particular sample is based on what the true motion was
        /// </summary>
        /// <param name="newState">The sample after being transformed</param>
        /// <param name="oldState">The sample before being transformed</param>
        /// <returns>A value between zero and one</returns>
        public double GetProbabilityOfSample(Pose2D newState, Pose2D oldState)
        {
            double w, v, g;

            if (oldState.Theta == newState.Theta)
            {
                // no rotation
                w = 0;
                v = _vel.Translation / _time;
                g = 0;
            }
            else
            {
                double mu, dTheta, x, y, r;
                mu = Math.Abs(((oldState.X - newState.X) * Math.Cos(oldState.Theta) + (oldState.Y - newState.Y) * Math.Sin(oldState.Theta)) /
                           (2 * ((oldState.Y - newState.Y) * Math.Cos(oldState.Theta) - (oldState.X - newState.X) * Math.Sin(oldState.Theta))));
                x = ((newState.X + oldState.X) / 2) + mu * (oldState.Y - newState.Y);
                y = ((newState.Y + oldState.Y) / 2) + mu * (newState.X - oldState.X);
                r = Math.Sqrt(Math.Pow(oldState.X - x, 2) + Math.Pow(oldState.Y - y, 2));
                dTheta = GetSmallestAngle(Math.Atan2(newState.Y - y, newState.X - x) - Math.Atan2(oldState.Y - y, oldState.X - x));

                if (oldState.Y == newState.Y)
                {
                    w = 0;
                    v = Math.Abs(oldState.X - newState.X);
                    Console.Out.WriteLine("no change in Y");
                } else {
                    w = (dTheta /_time);
                    v = w * r;
                }
                g = (GetSmallestAngleDifference(newState.Theta,oldState.Theta) / _time) - w;
                if (double.IsNaN(g)) {
                    Console.Out.WriteLine("calculated: x:" + x + " y:" + y + " r:" + r + " dTheta:" + dTheta + " v " + v + " mu: " + mu + " oldPos: " + oldState.X + "," + oldState.Y);
                }
            }

            double p1 = ProbabilityUtils.ProbabilityOfValInVariance(Math.Abs(_vel.Translation) - Math.Abs(v), _v_range) / _v_max; //tErr1 * Math.Pow(vel.Translation, 2) + tErr2 * Math.Pow(vel.Rotation, 2));
            double p2 = ProbabilityUtils.ProbabilityOfValInVariance(Math.Abs(_vel.Rotation) - w, _w_range) /_w_max;// rErr1 * Math.Pow(vel.Translation, 2) + rErr2 * Math.Pow(vel.Rotation, 2));
            double p3 = ProbabilityUtils.ProbabilityOfValInVariance(g, _g_range) / _g_max;// tErr3 * Math.Pow(vel.Translation, 2) + rErr3 * Math.Pow(vel.Rotation, 2));

            if (p1==0 || p2==0 || p3 ==0 || p1 > 1 || p2 > 1 || p3 > 1)
            {
                Console.Out.WriteLine("newState: " + newState.ToString() + " oldState: " + oldState.ToString() + " _vel: " + _vel + " g: " + g + " p1:" + p1 + " p2:" + p2 + " p3:" + p3 + " result:" + p1 * p2 * p3 + " w_max " + _w_max + " w_range " + _w_range);
            }
            return p1*p2*p3;
        }

        #region Private helper functions
        private static double GetSmallestAngle(double angle)
        {
            if (angle > Math.PI)
            {
                return 2 * Math.PI - angle;
            }
            else if (angle < -Math.PI)
            {
                return angle + Math.PI * 2;
            }
            else
            {
                return Math.Abs(angle);
            }
        }

        /// <summary>
        /// Finds the the absolute angle of the wedge between two angles
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns>The width of the wedge in radians</returns>
        private static double GetSmallestAngleDifference(double a, double b)
        {
            if (a >= 0 && b >= 0)
            {
                return Math.Abs(a - b);
            }
            else if (a < 0 && b < 0)
            {
                return Math.Abs(Math.Abs(a) - Math.Abs(b));
            }
            else
            {
                return GetSmallestAngle(Math.Abs(a) + Math.Abs(b));
            }
        }
        #endregion

        #region IMotionModel Members

        /// <summary>
        /// Transforms the state with no added noise
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        public Pose2D GetPerfectSample(Pose2D state)
        {
            return GetPerfectSample(state, _vel, new TimeSpan(0, 0, 1));
        }


        /// <summary>
        /// Used to  distinguish between model types
        /// </summary>
        /// <returns></returns>
        public string ModelType()
        {
            return "Velocity";
        }

        /// <summary>
        /// NOT IMPLEMENTED FOR THIS MODEL TYPE
        /// </summary>
        /// <param name="odoState"></param>
        /// <param name="odoStatePrev"></param>
        public void PrepareForSampling(Pose2D odoState, Pose2D odoStatePrev)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        #endregion
    }
}
